
/*******************************************************************
ArduPilot Flight controller, ver 0.8 by Magnus Leijonborg, magnus@leijonborg.se

This program is written to run in the ArduPilot hardware. When finished it will do the following:

Inputs:
*   IR1 rollangle from IR sensor
*   IR2 Pitchangle from IR sensor
* 	IR3 z-sensor for calibration
*	Mode control pin from Attiny 
* 	TWI in from Navigation Module
*   Calibration button
todo

* Always, clean up code
* Secure automatic mode when transmitter is switched off
* Calib sets Plevel and Rlevel to fixed values. should be measuer... 



Ardupilot pins

PC0 A0  IR1 in
PC1 A1	IR2 in
PC2 A2	IR3 in Z-sensor
PC3 A3
PC4 A4 SDA TWI
PC5 A5 SCL TWI

PD0	RX
PD1	TX

PD2 	Servo 1 in INT0
PD3 	Servo 2 in INT1

PD4	MISO?
PD5	Mode in

PD6 	pin D6  calbutton
PD7 	  

PB0	To mux
PB1	To mux
PB2	To mux
PB3	To mux not tested check in servo code that four servos are enabled


PB3	SHOULD BE SERVO (TO MUX)
PB4	LED bLUE LOW=ON
PB5	LED YELLOW LOW=ON

*********************************************************************/


// ----------------  Includes  -------------------------
#include <stdint.h>
#include <stdbool.h>
//#include <string.h>
#include <stdlib.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include "uart.h"
//#include "a2d.h"
#include <math.h>

#include "servo.h"
#include "misc.h"
#include "stabilization.h"
#include "TWI_slave.h"
#include <stdint.h>
#include <stdbool.h>
#include <util/delay.h>
#include <avr/eeprom.h>





// ----------------  Defines --------------------------

#define F_CPU 16000000UL

#define UART_BAUD_RATE      19200



// --------------- Global variables --------------------- 

// Misc
short 	i;
char rec;
//char sendFlag=0;

// Eeprom variables to store settings
int   EEMEM servoCenterEeprom[4];
int   EEMEM servoMaxEeprom[4];
int   EEMEM servoMinEeprom[4];

int max_ir=0;
char calib_enabled=1; 
char control_enabled=0; 


int   Rtarget, Ptarget;
double  Rollangle, Pitchangle;
int   roll_servo_value=0;
int   pitch_servo_value=0;

// Servo
int	servoPos[5];	 //	1000->2000
int	oldservoPos[5];	 //	1000->2000

int servoMax[5];	 // 1000
int servoMin[5];	 // 2000
int servoCenter[5];  //	1500


volatile uint16_t count0=0; 	// Counts R/C  pulse length on INT0
volatile uint16_t count1=0; 	// Counts R/C  pulse length on INT1

volatile int   pulse0=0;  
volatile int   pulse1=0;  


volatile unsigned int ping=0;
volatile unsigned int ping2=0;

char  AP,OAP; 	// 0=manual, 1=AP , 2=timeout

int   iTemp;
int   iTemp2;




// Communication 

int r;

volatile unsigned char cmd[7];
volatile char cmdCount=0;
volatile char cmdWaiting=0;
volatile unsigned char checksum=0;

char cmdstr[8];
unsigned long data=0;
unsigned long templ=0;
unsigned long tempint=0;
  

// --------------- END Global variables --------------------- 


// ***************  functions   *************

void packInt( unsigned char cmd, int data1,int data2) {
  templ=0;
  templ=data1;
  data=templ << 16;
  data=data+data2;

  cmdstr[1]=cmd+128;
  cmdstr[2]=(data && 15);
  tempint=data >> 4;
  cmdstr[3]=tempint && 127;
  tempint=data >> 11;
  cmdstr[4]=tempint && 127;
  tempint=data >> 18;
  cmdstr[5]=tempint && 127;
  tempint=data >> 25;
  cmdstr[6]=tempint && 127;
  cmdstr[7]=cmdstr[1]+cmdstr[2]+cmdstr[3]+cmdstr[4]+cmdstr[5]+cmdstr[6];
  cmdstr[7]=cmdstr[7]-128;
  uart_putc(cmdstr[1]);
  uart_putc(cmdstr[2]);
  uart_putc(cmdstr[3]);
  uart_putc(cmdstr[4]);
  uart_putc(cmdstr[5]);
  uart_putc(cmdstr[6]);
  uart_putc(cmdstr[7]);
 

}



void sendAck(char receivedCmd) {
	uart_putc(0x90);
	uart_putc(receivedCmd);
	uart_putc(0);
	uart_putc(0);
	uart_putc(0);
	checksum=0x90+receivedCmd;
	uart_putc(checksum);
}


void sendCmd(char cmdCode, char data1,char data2,char data3,char data4 ) {
	uart_putc(cmdCode);
	uart_putc(data1);
	uart_putc(data2);
	uart_putc(data3);
	uart_putc(data4);
	checksum=cmdCode+data1+data2+data3+data4;
	uart_putc(checksum);
}


void sendIntCmd(char cmdCode, int data1,int data2) {
	uart_putc(cmdCode);
	uart_putc(hi(data1));
	uart_putc(lo(data1));
	uart_putc(hi(data2));
	uart_putc(lo(data2));
	checksum=cmdCode+hi(data1)+lo(data1)+hi(data2)+lo(data2);
	uart_putc(checksum);
}



// ********************* Main ******************************
int main (void)
{ 


//----------  INITS --------------

// set directions of ports
DDRB =0b11111111; // Servo out ports
DDRC =0b00000000; // Analog in 


DDRD =0b00000010;
PORTD|=_BV(6);//Enable pull up

				

//Clear LEDs
BOFF;
YOFF;

// Uart
uart_init( UART_BAUD_SELECT(UART_BAUD_RATE,F_CPU) ); 

// Stabilization
initStabilization(1,0);

// start TIMER1, 16 bit timer free running. Used to measure servo pulse lengths
TCCR1B |= (1 << CS10); // Set up timer 

// Servos

servoStart();
servoSet(0, 1500);    // Center servo 1
servoSet(1, 1500);    // Center servo 2
servoSet(2, 1500);    // Center servo 3
// servoSet(3, 1500);    // Center servo 4




// Interrupts
// set D2 and D3 as inputs 
EICRA=5; // Enable Interrupt on INT1 at every change high/low. Used to capture R/C pulse for AP-control
EIMSK=3;  // Enables interrupt INT0 and INT1


// timer 0 setup. we want approx 100 hz 
// 16mhz / 256 / 1024 = 61

TCCR0A = 0b00000000;
TCCR0B = 4;  // set prescaler to 1024
TIMSK0 = 1;  // overflow interrupt enable


sei();    //Enable CPU interrupts


//---------------- END Inits -------------------- 






// Load settins from EEPROM

// IR

//  servo

for (i=1;i<=4;i++) {
  servoCenter[i]=eeprom_read_word(&servoCenterEeprom[i]);
  servoMax[i]=2000; //eeprom_read_word(&servoMaxEeprom);
  servoMin[i]=1000; //eeprom_read_word(&servoMinEeprom);
}

sendCmd(0,0,0,0,0);
sendCmd(1,0,0,0,0);
sendIntCmd(16,1,getPid(1));
sendIntCmd(16,2,getPid(2));
sendIntCmd(16,3,getPid(3));
sendIntCmd(16,4,getPid(4));
sendIntCmd(16,5,getPid(5));
sendIntCmd(16,6,getPid(6));

sendIntCmd(17,1,getIR(1));
sendIntCmd(17,2,getIR(2));
sendIntCmd(17,3,getIR(3));







//packInt(12,1,1);



// ******** Start forever loop  **********
// ***************************************
for (;;) {
	YON;
	BON;

  // Check serial port for new data 

 // Ceck for commands from PC
	r= uart_getc();
	if (r<256) {
		cmdCount++;
		cmd[cmdCount]=r;
		if (cmdCount>=6) {
			checksum=cmd[1]+cmd[2]+cmd[3]+cmd[4]+cmd[5];
			if (checksum==cmd[6]) {
				cmdWaiting=1;
				cmdCount=0;
				checksum=0;
			}
			else {
				cmdCount=0;
				checksum=0;
			}
		}
     }

	if (cmdWaiting==1) {
		
       	switch(cmd[1]) {
		case 1:  Rtarget=toInt(cmd[2],cmd[3]);
				 Ptarget=toInt(cmd[4],cmd[5]);
				 break;
		case 12: iTemp=toInt(cmd[2],cmd[3]);
				 iTemp2=toInt(cmd[4],cmd[5]);
				 setPid(iTemp,iTemp2);
	 			 iTemp2=getPid(iTemp);
	 			 sendIntCmd(16,iTemp,iTemp2);
			
				 break;
		case 13: iTemp=toInt(cmd[2],cmd[3]);
				 iTemp2=getPid(iTemp);
	 			 sendIntCmd(16,iTemp,iTemp2);
				 break;
		case 16: iTemp=toInt(cmd[2],cmd[3]);
				 iTemp2=toInt(cmd[4],cmd[5]);
				 setIR(iTemp,iTemp2);
	 			 iTemp2=getIR(iTemp);
	 			 sendIntCmd(17,iTemp,iTemp2);
				 break;

	
		} // switch
		cmdWaiting=0;
	} //if


if (ping==6) {  //128=1HZ  6 =20Hz

	// Read target values from servo 0 and servo 1	
	Rtarget=-(pulse0/16-1500)/8; 
	Ptarget=-(pulse1/16-1500)/8; 
	
	// Read rollangle and calculate servo deflection
	Rollangle=-get_roll_angle();
	roll_servo_value=do_roll_PID(Rtarget,Rollangle);
	if (control_enabled==1) 
		servoSet(1,1450+roll_servo_value); 
	else
		servoSet(1,1450); 


	// Read pitchangle and calculate servo deflection
	Pitchangle=get_pitch_angle();
	pitch_servo_value=do_pitch_PID(Ptarget, Pitchangle);
	if (control_enabled==1) 
		servoSet(2,1550+pitch_servo_value);
	else
	    servoSet(2,1550);

	ping=0;
}

if (ping2==24) {  //128=1HZ  6 =5Hz
	if (bit_is_set(PIND,4)) {  // Sednd data when AP is active
		sendIntCmd(12,Rtarget,Ptarget);
		sendIntCmd(13,(int)Rollangle,(int)Pitchangle);
		sendIntCmd(14,roll_servo_value,pitch_servo_value);
		
		// Send raw ir values instead ov servo values	
		iTemp=get_roll_raw();
		iTemp2=get_pitch_raw();
		//sendIntCmd(14,iTemp,iTemp2);
	}
	if (bit_is_clear(PIND,6) && (calib_enabled==1)) {  
		// do IR calibration
		max_ir=calibrate();
		sendIntCmd(15,max_ir,0);
		// disable calibration
		calib_enabled=0;
		// enable AP-control
		control_enabled=1;
	}
	
	ping2=0;
}


} //  ****** End forever loop ******

}  //  ************* End main ********




/************************************************************************
Interrupt handler that triggers when INT1 changes from low to high or
from high to low. 
L->H represents start of R/C pulse
H->L represents end of R/C pulse

The routine uses TIMER1 to calculate pulse length and to decide if AP is 
active or not.

TODO:
* Handle loss of signal and glitches

*************************************************************************/


ISR(INT1_vect)
{
if ((PIND & 0b00001000)==8) {
   // Start pulse
//	TCNT1=0;
	count1=TCNT1;  
    }
else {
   // End pulse 
	if (TCNT1<count1) {
	  pulse1=(TCNT1+(65536-count1));
	  
	  }
	else
	 pulse1=(TCNT1-count1);	

	}  // end End pulse
} // end INT1_vect


// ******************** END ISR(INT1_vect) *****************************



/************************************************************************
Interrupt handler that triggers when INT0 changes from low to high or
from high to low. 
L->H represents start of R/C pulse
H->L represents end of R/C pulse

The routine uses TIMER1 to calculate pulse length and to decide if AP is 
active or not.

TODO:
* Handle loss of signal and glitches

*************************************************************************/


ISR(INT0_vect)
{
if ((PIND & 0b00000100)==4) {
   // Start pulse
//	TCNT1=0;
	count0=TCNT1;  
    }
else {
   // End pulse 
	if (TCNT1<count0) {
	  pulse0=(TCNT1+(65536-count0));
	  
	  }
	else
	 pulse0=(TCNT1-count0);	

	}  // end End pulse
} // end INT1_vect


// ******************** END ISR(INT0_vect) *****************************

// **********************************************
// Interrupt when timer 0 overflows
// with 8Mhz and prescaler at 64 it triggers at 
// 122Hz
// **********************************************

ISR(TIMER0_OVF_vect) {
   ping++;   
   ping2++;
 }
